#include <stdio.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <sys/mman.h>
#include <string.h>
#include <stdlib.h>


#include "cbb_camera_ctrl.h"
#include "cbb_camera_process.h"


CbbCameraAckE_t CbbCameraQuePixelFormat(CameraHandle handle, CbbCameraCtrlDev_t *operate_p)
{
    if (NULL == handle || NULL == operate_p) {
        printf("Invalid Params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }

    printf("%s\r\n", __func__);
    CbbCameraHandle_t *camera_dev = (CbbCameraHandle_t *)handle;
    if (camera_dev->fd <= 0) {
        printf("camera_dev->fd invalid\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    
    //先查询设备能力,决定填充type
    struct v4l2_capability cap;
    //获取设备能力
    int ioctl_ret = ioctl(camera_dev->fd, VIDIOC_QUERYCAP, &cap);
    if (ioctl_ret < 0)
    {
        perror("ioctl VIDIOC_QUERYCAP failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    //
    struct v4l2_fmtdesc fmtdesc = {0};

    if(V4L2_CAP_VIDEO_CAPTURE & cap.capabilities) {
        printf("support V4L2_CAP_VIDEO_CAPTURE\r\n");
        fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    }
    else if(V4L2_CAP_VIDEO_CAPTURE_MPLANE & cap.capabilities) {
        printf("support V4L2_CAP_VIDEO_CAPTURE_MPLANE\r\n");
        fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    }
    else{
        printf("caps cannot support V4L2_CAP_VIDEO_CAPTURE_MPLANE or V4L2_CAP_VIDEO_CAPTURE\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }

    //查询格式
    fmtdesc.index = 0;
    while (ioctl(camera_dev->fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0)
    {
        if (fmtdesc.index < V4L2_PIXEL_FORMAT_MAX_NUM)
        {
            operate_p->camera_ctrl_u.pixel_format[fmtdesc.index] = fmtdesc.pixelformat;
        }
        #if 0
        printf("fmtdesc.index:%d\r\n", fmtdesc.index);
        printf("fmtdesc.flags:%d\r\n", fmtdesc.flags);
        printf("fmtdesc.description:%s\r\n", fmtdesc.description);
        printf("fmtdesc.reserved:%s\r\n", fmtdesc.reserved);
        printf("fmtdesc.fmt:%c%c%c%c\r\n", (fmtdesc.pixelformat & 0xFF),
                                    (fmtdesc.pixelformat >> 8 & 0xFF),
                                    (fmtdesc.pixelformat >> 16 & 0xFF),
                                    (fmtdesc.pixelformat >> 24 & 0xFF));
        #endif
        fmtdesc.index++;
    }

    return CBB_CAMERA_ACK_OK;
}

CbbCameraAckE_t CbbCameraQueAllInfo(CameraHandle handle, CbbCameraCtrlDev_t *operate_p)
{
    if (NULL == handle || NULL == operate_p)
    {
        printf("Invaild Params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    printf("%s\r\n", __func__);

    CbbCameraHandle_t *camera_dev = (CbbCameraHandle_t *)handle;
    if (camera_dev->fd <= 0) {
        printf("camera_dev->fd invalid\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }

    //获取设备能力
    struct v4l2_capability cap;
    int ioctl_ret = ioctl(camera_dev->fd, VIDIOC_QUERYCAP, &cap);
    if (ioctl_ret < 0) {
        perror("ioctl VIDIOC_QUERYCAP failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    //保存设备采集类型
    if(V4L2_CAP_VIDEO_CAPTURE & cap.capabilities) {
        camera_dev->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    }else if(V4L2_CAP_VIDEO_CAPTURE_MPLANE & cap.capabilities) {
        camera_dev->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    }else{
        printf("caps cannot support V4L2_CAP_VIDEO_CAPTURE_MPLANE or V4L2_CAP_VIDEO_CAPTURE\r\n");
        return CBB_CAMERA_ACK_ERROR;
    } 

    //查询当前格式,按实际设备能力区分type
    struct v4l2_format cur_fmt = {0};
    cur_fmt.type = camera_dev->type;

    ioctl_ret = ioctl(camera_dev->fd, VIDIOC_G_FMT, &cur_fmt);
    if (ioctl(camera_dev->fd, VIDIOC_G_FMT, &cur_fmt) < 0) {
        perror("ioctl VIDIOC_G_FMT failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    printf("num_planes:%d!!!\r\n", cur_fmt.fmt.pix_mp.num_planes);
    //结构体赋值
    if (cur_fmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
        operate_p->camera_ctrl_u.info_s.width = cur_fmt.fmt.pix_mp.width;
        operate_p->camera_ctrl_u.info_s.height = cur_fmt.fmt.pix_mp.height;
        operate_p->camera_ctrl_u.info_s.format = cur_fmt.fmt.pix_mp.pixelformat;
        operate_p->camera_ctrl_u.info_s.sizeimage = cur_fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
    }else{
        operate_p->camera_ctrl_u.info_s.width = cur_fmt.fmt.pix.width;
        operate_p->camera_ctrl_u.info_s.height = cur_fmt.fmt.pix.height;
        operate_p->camera_ctrl_u.info_s.format = cur_fmt.fmt.pix.pixelformat;
    }
    operate_p->camera_ctrl_u.info_s.capabilities = cap.capabilities;
    operate_p->camera_ctrl_u.info_s.device_caps = cap.device_caps;
    //摄像头版本
    snprintf(operate_p->camera_ctrl_u.info_s.camera_version, sizeof(operate_p->camera_ctrl_u.info_s.camera_version)-1, 
    "%u.%u.%u", (cap.version >> 16) & 0xff, (cap.version >> 18) & 0xff, cap.version & 0xff);
    printf("operate_p->camera_ctrl_u.info_s.camera_version:%s\r\n", operate_p->camera_ctrl_u.info_s.camera_version);

    return CBB_CAMERA_ACK_OK;
}

CbbCameraAckE_t CbbCameraSetConf(CameraHandle handle, CbbCameraCtrlDev_t *operate_p)
{
    if (NULL == handle || NULL == operate_p)
    {
        printf("Invaild Params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    printf("%s\r\n", __func__);

    CbbCameraHandle_t *camera_dev = (CbbCameraHandle_t *)handle;
    if (camera_dev->fd <= 0) {
        printf("camera_dev->fd invalid\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }

    //是否重新获取设备能力
    if (camera_dev->type <= 0 ) {
        struct v4l2_capability cap;
        int ioctl_ret = ioctl(camera_dev->fd, VIDIOC_QUERYCAP, &cap);
        if (ioctl_ret < 0) {
            perror("ioctl VIDIOC_QUERYCAP failed");
            return CBB_CAMERA_ACK_ERROR;
        }
        //保存设备采集类型
        if(V4L2_CAP_VIDEO_CAPTURE & cap.capabilities) {
            camera_dev->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        }else if(V4L2_CAP_VIDEO_CAPTURE_MPLANE & cap.capabilities) {
            camera_dev->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        }else{
            printf("caps cannot support V4L2_CAP_VIDEO_CAPTURE_MPLANE or V4L2_CAP_VIDEO_CAPTURE\r\n");
            return CBB_CAMERA_ACK_ERROR;
        } 
    }

    //根据设备能力选择类型
    struct v4l2_format format_conf = {0};
    if(camera_dev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
        format_conf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        format_conf.fmt.pix.width = operate_p->camera_ctrl_u.conf_s.width;
        format_conf.fmt.pix.height = operate_p->camera_ctrl_u.conf_s.height;
        format_conf.fmt.pix.pixelformat = operate_p->camera_ctrl_u.conf_s.format; //此格式需要转换?
    }else if(camera_dev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
        format_conf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        format_conf.fmt.pix_mp.width = operate_p->camera_ctrl_u.conf_s.width;
        format_conf.fmt.pix_mp.height = operate_p->camera_ctrl_u.conf_s.height;
        format_conf.fmt.pix_mp.pixelformat = operate_p->camera_ctrl_u.conf_s.format; //此格式需要转换?
    }

    //设置格式
    if (ioctl(camera_dev->fd, VIDIOC_S_FMT, &format_conf) < 0) {
        printf("ioctl VIDIOC_S_FMT failed\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    
    //设置帧率失败? 部分设备不支持设置
    // struct v4l2_streamparm stream_parm = {0};
    // stream_parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    // stream_parm.parm.capture.timeperframe.numerator = 1;
    // stream_parm.parm.capture.timeperframe.denominator = 20;
    // if (ioctl(camera_dev->fd, VIDIOC_S_PARM, &stream_parm) < 0)
    // {
    //     perror("ioctl VIDIOC_S_PARM failed");
    // }
    return CBB_CAMERA_ACK_OK;
}

CbbCameraAckE_t CbbStartCapture(CameraHandle handle)
{
    if (NULL == handle) {
        printf("Invaild Params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    
    CbbCameraHandle_t *camera_dev = (CbbCameraHandle_t *)handle;
    if (camera_dev->fd <= 0) {
        printf("camera_dev->fd invalid\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }

    //请求帧缓冲区 
    struct v4l2_requestbuffers req_buf = {0};
    req_buf.type = camera_dev->type;
    req_buf.count = 4; //帧缓冲器区个数 是否可由外部设置?
    req_buf.memory = V4L2_MEMORY_MMAP;

    if(ioctl(camera_dev->fd, VIDIOC_REQBUFS, &req_buf) < 0){
        perror("ioctl VIDIOC_REQBUFS failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    //映射缓冲区
    camera_dev->mmap_count = 0;
    struct v4l2_buffer map_buf = {0};
    map_buf.type = camera_dev->type;
    map_buf.memory = V4L2_MEMORY_MMAP;

    if (map_buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
    {
        struct v4l2_plane planes[VIDEO_MAX_PLANES] = {0};
        map_buf.length = VIDEO_MAX_PLANES;
        map_buf.m.planes = planes;
    }
    

    for (size_t i = 0; i < req_buf.count; i++) {

        map_buf.index = i;
        if(ioctl(camera_dev->fd, VIDIOC_QUERYBUF, &map_buf) < 0) {
            perror("ioctl VIDIOC_QUERYBUF failed");
            return CBB_CAMERA_ACK_ERROR;
        }

        if (map_buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
            for (size_t j = 0; j < map_buf.length; j++) {
                //单个plane映射
                camera_dev->mmap_buffer_size = map_buf.m.planes[j].length;
                camera_dev->mmap_array[camera_dev->mmap_count] = mmap
                (   
                    NULL, 
                    map_buf.m.planes[j].length, 
                    PROT_READ, 
                    MAP_SHARED, 
                    camera_dev->fd,
                    map_buf.m.planes[j].m.mem_offset
                );
                if(MAP_FAILED == camera_dev->mmap_array[camera_dev->mmap_count]) {
                    printf("Mmap camera device buffer failed\r\n");
                    continue;
                }
                camera_dev->mmap_count++;
                printf("camera_dev->mmap_count:%d\r\n", camera_dev->mmap_count);
                printf("map_buf.m.planes->length:%d\r\n", map_buf.m.planes[j].length);
                //入队
                if (ioctl(camera_dev->fd, VIDIOC_QBUF, &map_buf) < 0) {
                    perror("ioctl VIDIOC_QBUF failed");
                    continue;
                }
            }
        } else {
            camera_dev->mmap_buffer_size = map_buf.length;
            camera_dev->mmap_array[camera_dev->mmap_count] = mmap
            (   
                NULL, 
                map_buf.length, 
                PROT_READ, 
                MAP_SHARED, 
                camera_dev->fd,
                map_buf.m.offset
            );
            if(MAP_FAILED == camera_dev->mmap_array[camera_dev->mmap_count]) {
                printf("Mmap camera device buffer failed\r\n");
                continue;
            }
            camera_dev->mmap_count++;
            //入队
            if (ioctl(camera_dev->fd, VIDIOC_QBUF, &map_buf) < 0) {
                perror("ioctl VIDIOC_QBUF failed");
                continue;
            }
        }
    }
    
    //开启视频采集
    int type = req_buf.type;
    if(ioctl(camera_dev->fd, VIDIOC_STREAMON, &type) < 0) {
        perror("ioctl VIDIOC_QUERYBUF failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    return CBB_CAMERA_ACK_OK;
}

CbbCameraAckE_t CbbStopCapture(CameraHandle handle)
{
    if (NULL == handle) {
        printf("Invaild Params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    
    CbbCameraHandle_t *camera_dev = (CbbCameraHandle_t *)handle;
    if (camera_dev->fd <= 0) {
        printf("camera_dev->fd invalid\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    //停止视频流
    if(ioctl(camera_dev->fd, VIDIOC_STREAMOFF, &camera_dev->type) < 0) {
        perror("ioctl VIDIOC_QUERYBUF failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    //释放映射缓冲区
    for (size_t i = 0; i < camera_dev->mmap_count; i++) {
        if (MAP_FAILED == camera_dev->mmap_array[i]) {
            continue;
        } 
        munmap(camera_dev->mmap_array[i], camera_dev->mmap_buffer_size);
    }
    
    return CBB_CAMERA_ACK_OK;
}

CbbCameraAckE_t CbbCameraCtrlFlow(CameraHandle handle, CbbCameraCtrlDev_t *operate_p)
{
    if (NULL == handle || NULL == operate_p) {
        printf("Invaild Params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    printf("%s\r\n", __func__);

    CbbCameraAckE_t ack = CBB_CAMERA_ACK_ERROR;
    //开始采集
    if(operate_p->camera_ctrl_u.action == CBB_CAMERA_START) {
        ack = CbbStartCapture(handle);
    }
    //停止采集
    else if(operate_p->camera_ctrl_u.action == CBB_CAMERA_STOP) {
        ack = CbbStopCapture(handle);
    }
    //不支持的动作
    else {
        ack = CBB_CAMERA_ACK_ERROR;
    }

    return ack;
}

CbbCameraAckE_t CbbReadFrameData(CameraHandle handle, CbbCameraFrame_t *frame, hd_u16_t timeout_ms)
{
    if (NULL == handle || NULL == frame) {
        printf("Invaild Params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }
    CbbCameraHandle_t *camera_dev = (CbbCameraHandle_t *)handle;
    if (camera_dev->fd <= 0) {
        printf("camera_dev->fd invalid\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }

    struct v4l2_buffer read_buf= {0};
    read_buf.type = camera_dev->type;
    read_buf.memory = V4L2_MEMORY_MMAP;
    if (read_buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
        struct v4l2_plane planes[VIDEO_MAX_PLANES] = {0};
        read_buf.length = VIDEO_MAX_PLANES;
        read_buf.m.planes = planes;
    }

    struct timeval timeout = {0};
    fd_set fds;
    FD_ZERO(&fds);
    FD_SET(camera_dev->fd, &fds);
    timeout.tv_sec      = timeout_ms / 1000;
    timeout.tv_usec     = (timeout_ms - (timeout.tv_sec * 1000)) * 1000;
    int res = select(camera_dev->fd + 1, &fds, NULL, NULL, &timeout);
    if (-1 == res) {
        printf("Select camera device buffers error!\r\n");
        return CBB_CAMERA_ACK_ERROR;
    } else if (0 == res) {
        printf("Select camera device buffers timeout!\r\n");
        return CBB_CAMERA_ACK_ERROR;
    } else {
        if(!FD_ISSET(camera_dev->fd, &fds)) {
            printf("Select camera device buffers error!\r\n");
            return CBB_CAMERA_ACK_ERROR;
        } 
    }

    //获取数据 出队
    res = ioctl(camera_dev->fd, VIDIOC_DQBUF, &read_buf);
    if (res < 0) {
        perror("ioctl VIDIOC_DQBUF failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    //申请空间
    frame->data_addr = calloc(1, read_buf.m.planes->bytesused);
    //拷贝
    memcpy(frame->data_addr, camera_dev->mmap_array[read_buf.index], read_buf.m.planes->bytesused);
    //长度
    frame->len = read_buf.m.planes->bytesused;
    //保存时间戳
    frame->timestamp = read_buf.timestamp;
    //保存缓冲区索引
    frame->index = read_buf.index;

    return CBB_CAMERA_ACK_OK;
}

CbbCameraAckE_t CbbResetBuffer(CameraHandle handle, CbbCameraFrame_t frame, hd_u16_t index)
{
    if (NULL == handle) {
        printf("invaild params\r\n");
        return CBB_CAMERA_ACK_ERROR;
    }

    CbbCameraHandle_t *camera_dev = (CbbCameraHandle_t *)handle;

    struct v4l2_buffer reset_buf= {0};
    reset_buf.type     = camera_dev->type;
    reset_buf.memory   = V4L2_MEMORY_MMAP;
    if (reset_buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
        struct v4l2_plane planes[VIDEO_MAX_PLANES] = {0};
        reset_buf.length = VIDEO_MAX_PLANES;
        reset_buf.m.planes = planes;
    }

    reset_buf.index = frame.index;
    //入队
    int res = ioctl(camera_dev->fd, VIDIOC_QBUF, &reset_buf);
    if (res < 0) {
        perror("ioctl VIDIOC_QBUF failed");
        return CBB_CAMERA_ACK_ERROR;
    }

    return CBB_CAMERA_ACK_OK;
}